#ifndef _POLYGON_INTERSECTOR_H_
#define _POLYGON_INTERSECTOR_H_

#include <iostream>
#include <vector>
#include <limits>
#include <Eigen/Eigen>

class PolygonIntersector
{
public:
  struct Point
  {
    double x, y;
    Point() {}
    Point(double _x, double _y)
    {
      x = _x;
      y = _y;
    }
    Point(const Eigen::Vector2d &p)
    {
      x = p(0);
      y = p(1);
    };
  };

  typedef std::vector<Point> Points;

  PolygonIntersector(){};
  PolygonIntersector(const Points &polygon)
      : polygon_(polygon) {}

  void reset()
  {
    polygon_.clear();
    polyline_.clear();
  }

  void addPolygon(const std::vector<Eigen::Vector2d> &polygon)
  {
    polygon_.clear();
    size_t n = polygon.size();
    polygon_.resize(n);
    for (size_t i = 0; i < n; ++i)
    {
      polygon_[i].x = polygon[i](0);
      polygon_[i].y = polygon[i](1);
    }
  }

  void addPolyline(const std::vector<Eigen::Vector2d> &polyline)
  {
    size_t n = polyline.size();
    polyline_.resize(n);
    for (size_t i = 0; i < n; ++i)
    {
      polyline_[i].x = polyline[i](0);
      polyline_[i].y = polyline[i](1);
    }
  }

  bool isPointInPolygon(const Point &p) const
  {
    bool inside = false;
    for (size_t i = 0, j = polygon_.size() - 1; i < polygon_.size(); j = i++) {
      if ((polygon_[i].y > p.y) != (polygon_[j].y > p.y) &&
          p.x < (polygon_[j].x - polygon_[i].x) * (p.y - polygon_[i].y) /
                        (polygon_[j].y - polygon_[i].y) +
                    polygon_[i].x) {
        inside = !inside;
      }
    }
    return inside;
  }

  bool isInPolygon(const Eigen::Vector2d &p_eigen) const
  {
    Point p(p_eigen);
    return isPointInPolygon(p);
  }

  bool findIntersection(const Eigen::Vector2d &start,
                        const Eigen::Vector2d &end,
                        Eigen::Vector2d &intersection)
  {

    Point C(start);
    Point D(end);
    Point tmp;
    for (size_t j = 0; j < polygon_.size(); ++j)
    {
      Point A = polygon_[j];
      Point B = polygon_[(j + 1) % polygon_.size()];

      if (lineIntersect(A, B, C, D, tmp)) {
        intersection = {tmp.x, tmp.y};
        return true;
      }
    }
    std::cout << "[PIntersector]: intersection found failed !"
              << std::endl;

    return false;
  }

  bool findFirstIntersection(Point &intersection) const
  {
    if (!isPointInPolygon(polyline_[0])) {
      std::cout << "[PIntersector]: Poly Line start is not in the polygon."
                << std::endl;
      return false;
    }

    Points intersections;
    size_t n = polyline_.size();
    for (size_t i = 1; i < n; ++i)
    {
      if (!isPointInPolygon(polyline_[i])) {
        Point C = polyline_[i - 1];
        Point D = polyline_[i];
        for (size_t j = 0; j < polygon_.size(); ++j)
        {
          Point A = polygon_[j];
          Point B = polygon_[(j + 1) % polygon_.size()];

          if (lineIntersect(A, B, C, D, intersection)) {
            intersections.push_back(intersection);
          }
        }
        break;
      }
    }

    if (intersections.size() == 1) return true;
    if (intersections.size() > 1) deleteSamePoints(intersections);
    if (intersections.size() == 1) return true;

    std::cout << "[PIntersector]: intersection found failed !"
              << " #intersections = " << intersections.size() << std::endl;

    for (size_t i = 0; i < intersections.size(); ++i)
    {
      std::cout << "Intersection " << i << ": ("
                << intersections[i].x << ", "
                << intersections[i].y << ")" << std::endl;
    }

    return false;
  }

  bool getPolygon(std::vector<Eigen::Vector2d> &list)
  {
    if (polygon_.empty()) return false;

    size_t n = polygon_.size();
    list.resize(n);
    for (size_t i = 0; i < n; i++) {
      list[i] = {polygon_[i].x, polygon_[i].y};
    }

    return true;
  }

private:
  Points polygon_;
  Points polyline_;

  bool lineIntersect(
      const Point &A, const Point &B, const Point &C, const Point &D,
      Point &intersection) const
  {
    double a1 = B.y - A.y;
    double b1 = B.x - A.x;
    double a2 = D.y - C.y;
    double b2 = D.x - C.x;
    // 线段AB和线段CD的交点
    double denominator = a1 * b2 - b1 * a2;

    // 如果分母为0，线段平行或共线，没有交点
    if (denominator == 0.0) {
      return false;
    }

    double ua = (b1 * (C.y - A.y) - a1 * (C.x - A.x)) / denominator;
    double ub = (b2 * (C.y - A.y) - a2 * (C.x - A.x)) / denominator;

    // 如果ua和ub都位于[0,1]之间，那么线段相交
    if (ua >= 0.0 && ua <= 1.0 && ub >= 0.0 && ub <= 1.0) {
      // 计算交点
      intersection = Point{C.x + ua * b2, C.y + ua * a2};
      return true;
    }

    // 否则，线段不相交
    return false;
  }

  void deleteSamePoints(Points &points, double epsilon = 1e-6) const
  {
    for (size_t i = 0; i < points.size(); ++i)
    {
      for (size_t j = i + 1; j < points.size();)
      {
        if (std::abs(points[i].x - points[j].x) < epsilon &&
            std::abs(points[i].y - points[j].y) < epsilon) {
          points.erase(points.begin() + j);
        }
        else {
          ++j;
        }
      }
    }
  }
};

#endif